﻿using CommonTools;
using FSUIPC;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Configuration;
using System.Linq;
using System.Net;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;

namespace SimWithP3d.Vehicle
{
    public class Pa28 : VehicleDevice
    {
        public Pa28() : base()
        {
            try
            {
                base.UdpInit(delegate () { ReceiveMessage(); });
                InitTimer();
            }
            catch (Exception ex)
            {
                App.RecordErrorLog(ex);
            }
        }
        #region PA28偏移量
        #region//顶部开关面板
        public static Offset<byte> Engine_Primer = new Offset<byte>("PA28_Other", 0x3100, true);//发动机启动注油器
        public static Offset<int> Master_Battery_Switch = new Offset<int>("PA28_Other", 0x3102);//主电池开关
        public static Offset<byte> Alternator = new Offset<byte>("PA28_Other", 0x3101, true);//交流发电机开关
        public static Offset<int> Fuel_Pump = new Offset<int>("PA28_Other", 0x3B98, true);//燃油泵开关
        public static Offset<short> Magneto = new Offset<short>("PA28_Other", 0x0892);//礠电机Prop: 0=Off, 1=right, 2=Left, 3=Both, 4=Star
        public static Offset<short> Lights_Switch = new Offset<short>("PA28_Other", 0x0D0C);//灯开关1=Nav, 2=Bcn, 4=Land, 8=Taxi, 16=Strobe, 32=Panel, 64=Recognition, 128=Wing, 256=Logo, 512=Cabin
        public static Offset<int> Avionics_Master = new Offset<int>("PA28_Other", 0x2E80, true);//主要电子设备开关
        public static Offset<int> GearUpDown = new Offset<int>("PA28_Other", 0x0BE8);//起落架
        #endregion
        public static Offset<int> Autopilot_Switch = new Offset<int>("PA28_Other", 0x07BC);//自动驾驶仪开关:0=Off, 1=Active
        //操作面板
        public static Offset<byte> Elt_Switch = new Offset<byte>("PA28_Other", 0x341D, true);//应急发射转换开关
        public static Offset<byte> Pitot_Heat = new Offset<byte>("PA28_Other", 0x029C);//空速管加温
        public static Offset<byte> Fuel_Pump_Break = new Offset<byte>("PA28_Other", 0x3104);//燃油泵1 up,0 down
        public static Offset<double> Avionics_Master_Bus = new Offset<double>("PA28_Other", 0x2850);//电量
        public static Offset<short> Mixture = new Offset<short>("PA28_Axis", 0x0890);//油门混合比杆
        public static Offset<short> Throttle = new Offset<short>("PA28_Axis", 0x088C);//油门杆
        public static Offset<byte> Alt_Static_Air = new Offset<byte>("PA28_Other", 0x029B);//Alternate static air source (0=off, 1=on)
        public static Offset<short> Rudder_Trim = new Offset<short>("PA28_Other", 0x0C04);//左右配平左负右正
        public static Offset<short> Fuel_Tank_Selector = new Offset<short>("PA28_Other", 0x0AF8);//油箱选择OFF-L-R
        public static Offset<int> Fuel_Value = new Offset<int>("PA28_Other", 0x3590);//发动机1燃油阀，1 =打开，0 =关闭
        public static Offset<short> Propeller_Control = new Offset<short>("PA28_Other", 0x088E);//螺旋桨控制
        public static Offset<short> Left_Brake = new Offset<short>("PA28_Other", 0x0BC4, true);//左刹车
        public static Offset<short> Right_Brake = new Offset<short>("PA28_Other", 0x0BC6, true);//右刹车
        public static Offset<int> Flaps = new Offset<int>("PA28_Other", 0x0BDC, true);//FLap Position 0、1、2、3  0 ，5461、 10922 、 16383
        public static Offset<short> Elevator_Trim = new Offset<short>("PA28_Axis", 0x0BC0);//上下配平轮上正下负
        public static Offset<short> Rudder_Axis = new Offset<short>("PA28_Axis", 0x0BBA, true);//脚蹬方向舵 左负右正
        public static Offset<short> Elevator_Axis = new Offset<short>("PA28_Axis", 0x0BB2, true);//操作杆俯仰 仰正俯负
        public static Offset<short> Aileron_Axis = new Offset<short>("PA28_Axis", 0x0BB6, true);//操作杆翻滚 左负右正 
        public static Offset<short> Parking = new Offset<short>("PA28_Other", 0x0BC8, true);//手刹Parking brake: 0=off, 32767=on
        public static Offset<short> Carb_heat = new Offset<short>("PA28_Other", 0x08B2, true);//化油器热控制
        public static Offset<short> Pause = new Offset<short>("PA28_Other", 0x0262, true);//冻结


        //发动机数据相关
        public static Offset<short> Hour_Meter = new Offset<short>("PA28_Other", 0x0898);//计时器，发动机大于1转开始计数
        public static Offset<short> Hour_Meter_tmp = new Offset<short>("PA28_Other", 0x08C8);//用户计算发动机转速
        public static Offset<byte> Master_Warning = new Offset<byte>("PA28_Other", 0x036C);//失速警告
        public static Offset<byte> Master_Caution = new Offset<byte>("PA28_Other", 0x036D);//超速警告
        //Fail
        public static Offset<byte> COM1_Break = new Offset<byte>("PA28_Other", 0x0B68);//0ok,1fail
        public static Offset<byte> COM2_Break = new Offset<byte>("PA28_Other", 0x0B68);//0ok,1fail
        public static Offset<byte> XPNDR_ADS = new Offset<byte>("PA28_Other", 0x0B6F);//0ok,1fail
        public static Offset<byte> ADF_TURN = new Offset<byte>("PA28_Other", 0x0B64);//0ok,1fail
        public static Offset<byte> Turn_Coord = new Offset<byte>("PA28_Other", 0x0B72);//0ok,1fail

        public static Offset<byte> ALT_Break = new Offset<byte>("PA28_Other", 0x3101);//1ok,0fail

        //运动平台相关数据
        public static Offset<int> Pitch_Angle = new Offset<int>("MotionPlatform", 0x0578, false);   // 俯仰角 PLANE PITCH DEGREES     Radians        //*360/(65536*65536)   dgree    下加上减  左加右减
        public static Offset<int> Bank_Angle = new Offset<int>("MotionPlatform", 0x057C, false);//倾斜角 PLANE BANK DEGREES      Radians 
        public static Offset<uint> Heading_Angle = new Offset<uint>("MotionPlatform", 0x0580, false);//相对正北方向 PLANE HEADING DEGREES TRUE    Radians

        public static Offset<double> RollVel = new Offset<double>("MotionPlatform", 0x30B0);//ROTATION VELOCITY BODY X 
        public static Offset<double> PitchVel = new Offset<double>("MotionPlatform", 0x30A8); //ROTATION VELOCITY BODY Y  Feet per second       //rads/sec
        public static Offset<double> YawVel = new Offset<double>("MotionPlatform", 0x30B8);//ROTATION VELOCITY BODY Z 

        public static Offset<double> XAcc = new Offset<double>("MotionPlatform", 0x3060);  //ACCELERATION BODY X  Feet per second squared    //ft/sec/sec
        public static Offset<double> YAcc = new Offset<double>("MotionPlatform", 0x3068);//ACCELERATION BODY Y 
        public static Offset<double> ZAcc = new Offset<double>("MotionPlatform", 0x3070);// ACCELERATION BODY Z 

        public static Offset<double> PitchAcc = new Offset<double>("MotionPlatform", 0x3078, false);     //radians/sec/sec
        public static Offset<double> RollAcc = new Offset<double>("MotionPlatform", 0x3080, false);
        public static Offset<double> YawAcc = new Offset<double>("MotionPlatform", 0x3088, false);

        #endregion
        #region 定时器相关
        /// <summary>
        /// 定时发送操纵杆数据到P3D
        /// </summary>
        static System.Timers.Timer WriteAxisTimer = new System.Timers.Timer();
        /// <summary>
        /// 定时写入除驾驶杆外的其它数据到P3D
        /// </summary>
        static System.Timers.Timer WriteOtherTimer = new System.Timers.Timer();
        /// <summary>
        /// 定时读取运行平台相关速度数据
        /// </summary>
        static System.Timers.Timer MotionPlatformTimer = new System.Timers.Timer();
        /// <summary>
        /// 初始化定时器
        /// </summary>
        private void InitTimer()
        {
            WriteAxisTimer.Elapsed += WriteAxisHander;
            WriteAxisTimer.Interval = 30;
            WriteAxisTimer.Start();

            WriteOtherTimer.Elapsed += WriteOtherHander;
            WriteOtherTimer.Interval = 100;
            WriteOtherTimer.Start();

            MotionPlatformTimer.Elapsed += MotionPlatformTimer_Elapsed;
            MotionPlatformTimer.Interval = 40;
        }

        private void MotionPlatformTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            UpdateValue("MotionPlatform");
            OutSimPack outSimPack = new OutSimPack();
            outSimPack.nCmdID = 103;
            outSimPack.Xa = Convert.ToSingle(XAcc.Value);
            outSimPack.Ya = Convert.ToSingle(YAcc.Value);
            outSimPack.Za = Convert.ToSingle(ZAcc.Value);
            outSimPack.XrV = Convert.ToSingle(RollVel.Value);
            outSimPack.YrV = Convert.ToSingle(PitchVel.Value);
            outSimPack.ZrV = Convert.ToSingle(YawVel.Value);
            outSimPack.Xr = Convert.ToSingle(Pitch_Angle.Value * 360.0 / 65536.0 / 65536.0 * (Math.PI / 180));
            outSimPack.Yr = Convert.ToSingle(Bank_Angle.Value * 360.0 / 65536.0 / 65536.0 * (Math.PI / 180));
            outSimPack.Zr = Convert.ToSingle(Heading_Angle.Value * 360.0 / 65536.0 / 65536.0 * (Math.PI / 180));
            byte[] buf = CommonTool.StructToBytes(outSimPack);
            vehicleDeviceUdpClient.Send(buf, buf.Length, motionPlatformIPEndPoint);
        }

        /// <summary>
        /// 定时发送操纵杆数据到P3D
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        public void WriteAxisHander(object sender, EventArgs e)
        {
            UpdateValue("PA28_Axis");
        }
        /// <summary>
        /// 定时写入除驾驶杆外的其它数据到P3D
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        public void WriteOtherHander(object sender, EventArgs e)
        {
            UpdateValue("PA28_Other");
            //ElevatorTrimSendTimeData();
            TachometerStartTimes();
        }
        #endregion

        #region UDP接收处理
        /// <summary>
        /// 处理接收到的UDP消息
        /// </summary>
        /// <returns></returns>
        public override ThreadStart ReceiveMessage()
        {
            IPEndPoint remotePoint = null;
            while (true)
            {
                try
                {
                    byte[] rData = vehicleDeviceUdpClient.Receive(ref remotePoint);
                    if (App.debug)
                    {
                        App.realDataViewModel.LowerDevicesDataDeal(rData);
                    }
                    else
                    {
                        LowerDevicesDataDeal(rData);
                    }
                }
                catch (Exception ex)
                {
                    App.RecordErrorLog(ex);
                    Console.WriteLine("在" + ex.StackTrace.Split('在').Last() + ex.Message);
                }
            }
        }
        /// <summary>
        /// 处理下位机发送过来的消息
        /// </summary>
        /// <param name="rData"></param>
        private void LowerDevicesDataDeal(byte[] rData)
        {
            try
            {
                int nodeId;//区域编号
                int mark;//代号（2字节）
                ParseBytesToStr(rData, out nodeId, out mark);

                byte[] datas = new byte[6];
                Array.ConstrainedCopy(rData, 6, datas, 0, 6);

                switch (nodeId)
                {
                    case 01:
                        switch (mark)
                        {
                            case 1:
                                SwitchPanel1(datas);
                                break;
                            case 2:
                                SwitchPanel2(datas);
                                break;
                            case 5001:
                                byte[] throttle = new byte[3];
                                Array.ConstrainedCopy(datas, 0, throttle, 0, 3);
                                Throttle.Value = (short)ByteToHexStr(throttle);//油门杆
                                break;
                            case 5002:
                                byte[] mixture = new byte[3];
                                Array.ConstrainedCopy(datas, 0, mixture, 0, 3);
                                Mixture.Value = (short)ByteToHexStr(mixture);//油门混合比杆
                                break;
                            case 5003:
                                App.log.Info("配平数据" + BitConverter.ToString(datas).Replace("-", ""));

                                byte[] rudder = new byte[3];
                                Array.ConstrainedCopy(datas, 0, rudder, 0, 3);

                                byte[] elevator = new byte[3];
                                Array.ConstrainedCopy(datas, 3, elevator, 0, 3);
                                Rudder_Trim.Value = (short)(ByteToHexStr(rudder) - 16383);
                                Elevator_Trim.Value = (short)(ByteToHexStr(elevator) - 16383);
                                break;
                            default:
                                break;
                        }
                        break;
                    case 02:
                        byte[] hight3 = new byte[3];
                        Array.ConstrainedCopy(datas, 0, hight3, 0, 3);

                        byte[] low3 = new byte[3];
                        Array.ConstrainedCopy(datas, 3, low3, 0, 3);
                        switch (mark)
                        {
                            case 1000://运行平台控制
                                OutSimPack outSimPack = new OutSimPack();
                                switch (datas[0])
                                {
                                    case 0://无动作
                                        break;
                                    case 1://上升
                                        outSimPack.nCmdID = 101;
                                        MotionPlatformTimer.Start();
                                        break;
                                    case 2://下降
                                        outSimPack.nCmdID = 102;
                                        MotionPlatformTimer.Stop();
                                        break;
                                    case 3://停止
                                        outSimPack.nCmdID = 104;
                                        MotionPlatformTimer.Stop();
                                        break;
                                    default:
                                        break;
                                }
                                byte[] buf = CommonTool.StructToBytes(outSimPack);
                                vehicleDeviceUdpClient.Send(buf, buf.Length, motionPlatformIPEndPoint);
                                break;
                            case 5004://脚蹬数据
                                Rudder_Axis.Value = (short)(ByteToHexStr(hight3) - 16383);
                                break;
                            case 5005://驾驶杆数据
                                Aileron_Axis.Value = (short)(ByteToHexStr(hight3) - 16383);
                                Elevator_Axis.Value = (short)(ByteToHexStr(low3) - 16383);
                                break;
                            case 5007://踏板数据
                                Left_Brake.Value = (short)ByteToHexStr(hight3);
                                Right_Brake.Value = (short)ByteToHexStr(low3);
                                break;
                            default:
                                break;
                        }
                        break;
                    default:
                        break;
                }
            }
            catch (Exception ex)
            {
                App.RecordErrorLog(ex);
            }

        }
        #endregion

        #region 下位机接收处理方法

        public byte apBKB = 3;//ap状态缓存，用于比较ap状态是否发生变化
        /// <summary>
        /// 进入自动模式后，定时发送配平轮数据到下位机
        /// </summary>
        private void ElevatorTrimSendTimeData()
        {
            byte[] data = new byte[6];
            if (Autopilot_Switch.Value == 1)
            {
                if (apBKB != 1)
                {
                    apBKB = 1;
                    //TODO 未按协议实现
                    SendData(0x01, 10001, data);
                }
                //TODO 未按协议实现
                SendData(0x01, 10001, data);
            }
            else
            {
                if (apBKB != 0)
                {
                    apBKB = 0;
                    //TODO 未按协议实现
                    SendData(0x01, 10001, data);
                }
            }
        }
        /// <summary>
        /// mark1 开关面板 masterALT BAT 开关,avionics BUS1 bus2开关，灯光组开关、FUEL PUMP，PITOT HEAT，CABIN PEWR，STBY BATT
        /// </summary>
        private void SwitchPanel1(byte[] datas)
        {
            BitArray panel_1 = new BitArray(datas);
            //蓄电池
            byte Master_Battery_Switch_Byte = (byte)(panel_1[0] ? 1 : 0);
            if (Master_Battery_Switch.Value != Master_Battery_Switch_Byte)
            {
                Master_Battery_Switch.Value = Master_Battery_Switch_Byte;
            }
            //发动机启动注油器
            Engine_Primer.Value = (byte)(panel_1[1] ? 1 : 0);
            //钥匙开关
            Magneto.Value = panel_1[2] ? (short)4 : (short)3;
            Magneto.Value = panel_1[12] ? (short)1 : Magneto.Value;
            Magneto.Value = panel_1[13] ? (short)2 : Magneto.Value;
            Magneto.Value = panel_1[12] && panel_1[13] ? (short)3 : Magneto.Value;

            if (Avionics_Master_Bus.Value < 17 && Magneto.Value == 4)
            {
                if (SimControlAircraft.simConnect == null)
                    SimControlAircraft.connectP3d();
                if (SimControlAircraft.simConnect != null)
                    SimControlAircraft.simTaskEventID("ELECTRIC_GPU_SWITCH_TOGGLE");
            }
            //空速管加温
            Pitot_Heat.Value = (byte)(panel_1[3] ? 1 : 0);
            //自动驾驶仪开关
            Autopilot_Switch.Value = (byte)(panel_1[4] ? 1 : 0);
            //油箱选择 2=Left, 3=Right
            Fuel_Tank_Selector.Value = (byte)(panel_1[5] ? 3 : 2);
            //油箱选择 1=All
            Fuel_Tank_Selector.Value = (byte)(panel_1[6] ? 1 : Fuel_Tank_Selector.Value);
            //应急发射转换开关
            Elt_Switch.Value = (byte)(panel_1[7] ? 1 : 0);
            //灯开关1=Nav, 2=Bcn, 4=Land, 8=Taxi, 16=Strobe, 32=Panel, 64=Recognition, 128=Wing, 256=Logo, 512=Cabin
            short Lights_Switch_Value = 0;
            if (panel_1[8])
            {
                Lights_Switch_Value += 512;
            }
            if (panel_1[9])
            {
                Lights_Switch_Value += 16;
            }
            if (panel_1[10])
            {
                Lights_Switch_Value += 1;
            }
            if (panel_1[11])
            {
                Lights_Switch_Value += 4;
            }
            Lights_Switch.Value = Lights_Switch_Value;
            //燃油泵开关
            Fuel_Pump.Value = (byte)(panel_1[14] ? 1 : 0);
            //交流发电机
            Alternator.Value = (byte)(panel_1[15] ? 1 : 0);
            //FLap Position 0、1、2、3 对应数值  0 ，5461、 10922 、 16383
            Flaps.Value = panel_1[18] ? 5461 : 0;
            Flaps.Value = panel_1[19] && !panel_1[18] ? 10922 : Flaps.Value;
            Flaps.Value = panel_1[19] && panel_1[18] ? 16383 : Flaps.Value;
            //化油器热控制
            Carb_heat.Value = panel_1[20] ? (short)1 : (short)0;
            //手刹Parking brake: 0=off, 32767=on
            Parking.Value = panel_1[21] ? (short)32767 : (short)0;
            //主要电子设备开关
            Avionics_Master.Value = panel_1[22] ? (short)1 : (short)0;
            //起落架
            GearUpDown.Value = panel_1[25] ? (short)1 : (short)0;
            //模拟器暂停/运行
            Pause.Value = panel_1[26] ? (short)1 : (short)0;
        }
        /// <summary>
        /// mark2 开关面板 masterALT BAT 开关,avionics BUS1 bus2开关，灯光组开关、FUEL PUMP，PITOT HEAT，CABIN PEWR，STBY BATT
        /// </summary>
        private void SwitchPanel2(byte[] datas)
        {

        }
        /// <summary>
        /// 将下位机发送上来的操作杆，脚蹬数据进行转换，axis为输入X坐标值，输出为值为Y坐标值
        /// </summary>
        /// <param name="xAxis">正常发送到P3D的数据，X坐标轴值</param>
        /// <param name="turningPointY">将axis进行曲线放大结束点Y值</param>
        /// <param name="userMax_yAxis">将axis进行放大后，自定义允许Y轴最大值，默认最大16383</param>
        /// <param name="level">第一段放大曲线的幅度等级，1为不放大</param>
        /// <returns></returns>
        private double ConvertAxis(double xAxis, double turningPointY = 10000, double userMax_yAxis = 16383, int level = 1)
        {
            double a = Math.Sqrt(Math.Abs(xAxis) / 16383) * 16383 * level;
            if (a > turningPointY)
            {
                double turningPointxAxis = Math.Pow((turningPointY / 16383 / level), 2) * 16383;

                a = turningPointY + (16383 - turningPointY) / (16383 - turningPointxAxis) * (Math.Abs(xAxis) - turningPointxAxis);//直线
                //a = turningPointY + Math.Sqrt(Math.Abs(Math.Abs(xAxis) - turningPointxAxis) / (16383 - turningPointxAxis)) * (16383 - turningPointY);//曲线
            }
            if (a > userMax_yAxis)
            {
                a = userMax_yAxis;
            }
            if (xAxis >= 0)
            {
                return a;
            }
            else
            {
                return -a;
            }
        }
        /// <summary>
        /// 发动机转速超过1开始向下位机发送计时
        /// </summary>
        private void TachometerStartTimes()
        {
            byte[] data = new byte[6];
            BitArray bits = new BitArray(data);
            int rotateSpeed = Hour_Meter.Value * Hour_Meter_tmp.Value / 65536;//发动机转速
            bits[7]= rotateSpeed >= 1 ? true : false;
            bits[6] = Master_Warning.Value == 1 ? true : false;
            bits[5] = Master_Caution.Value == 1 ? true : false;
            bits.CopyTo(data, 0);
            SendData(0x01, 10001, data);
        }
        #endregion
    }
}
